# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt
#import sviewgui.sview as sv
import pandas as pd
import sys
import numpy as np
import scipy as sy
from scipy.optimize import root,fsolve
import pandas as pd
import sympy
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
class 四杆机构等效力矩():
    def __init__(self):
        self.l1=0.4;self.l2=1;self.l3=0.7;self.ad=1.2;self.rc1=0.2
        self.rc2=0.5
        self.rc3=0.35
        self.m1=1.2
        self.m2=3
        self.m3=2.2
        self.rp2=0.75
        self.fx2=5
        self.mm1=25
        self.mm3=5
        self.g=9.8
        self.初始=[np.pi/3,np.pi/3]
        self.thea=0
        self.res=self.质心矩阵重写求解雅可比()
        self.主动雅克比=self.主动力矩阵重写求解雅可比()
    def 力矩对比(self,thea1,thea2,thea3):
        力矩=self.mm1+self.fx2*self.l1*np.sin(thea1)-self.m1*self.g*self.rc1*np.cos(thea1)-self.m2*self.g*self.l1*np.cos(thea1) \
        +(self.fx2*self.rp2*np.sin(thea2)-self.m2*self.g*self.rc2*np.cos(thea2)) \
        *(self.l1*np.sin(thea1-thea3)/(self.l2*np.sin(thea3-thea2)))-(self.m3*self.g*self.rc3*np.cos(thea3)+self.mm3) \
        *(self.l1*np.sin(thea1-thea2)/(self.l3*np.sin(thea3-thea2)))
        return 力矩
    def 四杆机构向量方程(self,x):
        return np.array([self.l1 * np.cos(self.thea)+self.l2 * np.cos(x[0]) -self.l3 * np.cos(x[1]) -self.ad * np.cos(0), \
                         self.l1 * np.sin(self.thea)+self.l2 * np.sin(x[0]) -self.l3 * np.sin(x[1]) -self.ad * np.sin(0)])
    def 解方程(self):
        sol3_fsolve = fsolve(self.四杆机构向量方程, self.初始)
        return sol3_fsolve
    def 求解(self):
        数据=pd.DataFrame()
        角度=np.linspace(0,4*np.pi,1001)
        力矩对比=np.linspace(0,4*np.pi,1001)
        位移=np.linspace(0,4*np.pi,1001)
        #时间=np.linspace(0,2*np.pi/self.角速度,10001)#时间
        角度1 = np.linspace(0, 4 * np.pi, 1001)
        角度2 = np.linspace(0, 4 * np.pi, 1001)
        力矩改写乘法=np.linspace(0, 4 * np.pi, 1001)
        for i in range(len(角度)):
            self.thea=角度[i]
            shuju=self.解方程()
            self.初始=shuju
            角度1[i]=shuju[0]
            角度2[i]=shuju[1]
            #力矩普通乘法[i]=力矩普通乘法1
            力矩对比[i]=self.力矩对比(角度[i], 角度1[i], 角度2[i])
            力矩改写乘法[i]=float(self.力矩改写乘法(float(角度[i]), float(角度1[i]), float(角度2[i])))
        数据['角度']=角度
        数据['角度1'] = 角度1
        数据['角度2'] = 角度2
        数据['力矩对比'] = 力矩对比
        数据['力矩改写乘法'] = 力矩改写乘法
        #力矩普通乘法=self.力矩普通乘法(角度,角度1,角度2)
        fig, ax = plt.subplots()
        ax.set_title("角度-公式改编计算")
        ax.plot(数据['角度'],数据['力矩对比'])
        fig, ax = plt.subplots()
        ax.set_title("角度-计算机力矩计算")
        ax.plot(数据['角度'],数据['力矩改写乘法'])
        #plt.plot(角度,力矩普通乘法)
        return 数据
    def 质心矩阵(self,角度,角度1,角度2):
        return np.array([[-self.rc1*np.sin(角度)+self.rc1*np.cos(角度)*1j,0,0], \
                         [-self.l1*np.sin(角度)+self.l1*np.cos(角度)*1j,-self.rc2*np.sin(角度1)+self.rc2*np.cos(角度1)*1j,0], \
                         [0,0,-self.rc3*np.sin(角度2)+self.rc3*np.cos(角度2)*1j]])
    def 主动力雅克比(self,角度,角度1,角度2):
        return np.array([-self.l1*np.sin(角度)+self.l1*np.cos(角度)*1j,-self.rp2*np.sin(角度1)+self.rp2*np.cos(角度1)*1j,0])
    def 约束矩阵1(self,角度,角度1,角度2):
        return np.array([[(self.l1*np.sin(角度-角度2))/(self.l2*np.sin(角度2-角度1))], \
                         [(self.l1*np.sin(角度-角度1))/(self.l3*np.sin(角度2-角度1))]])
    def 约束矩阵(self,角度,角度1,角度2):
        return np.array([[1], \
                         [(self.l1*np.sin(角度-角度2))/(self.l2*np.sin(角度2-角度1))], \
                         [(self.l1*np.sin(角度-角度1))/(self.l3*np.sin(角度2-角度1))]])
    def 重力矩阵(self):
        return np.array([-self.m1*self.g*1j,-self.m2*self.g*1j,-self.m3*self.g*1j])
    def 主动力矩阵(self):
        return  np.array([-self.fx2])
    def 力矩矩阵(self):
        return np.array([+self.mm1,0,-self.mm3])
    def 力矩普通乘法(self,角度,角度1,角度2):
        力矩普通乘法=(self.重力矩阵().dot(self.质心矩阵(角度,角度1,角度2).T.dot(self.约束矩阵(角度,角度1,角度2))))+ \
               (self.主动力矩阵().dot(self.主动力雅克比(角度,角度1,角度2).T.dot(self.约束矩阵(角度,角度1,角度2))))+self.力矩矩阵().dot(self.约束矩阵(角度,角度1,角度2))
        return 力矩普通乘法
    def 力矩特殊乘法(self,角度,角度1,角度2):
        力矩特殊乘法 = np.mat(self.重力矩阵()).T.H.dot(np.mat(self.质心矩阵(角度, 角度1, 角度2)).dot(self.约束矩阵(角度, 角度1, 角度2)))
        return 力矩特殊乘法
    def 质心矩阵重写求解雅可比(self):
        a, b, c, a1, b1, c1, x, y,z = sympy.symbols("a b c a1 bi c1 x y z",real=True)
        a = self.rc1*sympy.cos(x)
        a1 = self.rc1 * sympy.sin(x)
        b =self.l1*sympy.cos(x)+self.rc2*sympy.cos(y)
        b1 =self.l1*sympy.sin(x)+self.rc2*sympy.sin(y)
        c =  self.ad*sympy.cos(0)+self.rc3*sympy.cos(z)
        c1 = self.ad*sympy.sin(0)+self.rc3*sympy.sin(z)
        funcs = sympy.Matrix([a, a1, b, b1, c, c1])
        #print(funcs)
        args = sympy.Matrix([x,y,z])
        res = funcs.jacobian(args)
        return res
    def 改写质量心矩阵(self,角度1,角度2,角度3):
        x, y ,z= sympy.symbols("x y z", real=True)
        result = self.res.subs([(x,角度1),(y,角度2),(z,角度3)])
        result=result.evalf()
        #g_func = lambdify((5,6),res)
        result=np.array(result)
        #print(result)
        #print(result)
        return result

    def 主动力矩阵重写求解雅可比(self):
        a, b, c, a1, b1, c1, x, y,z = sympy.symbols("a b c a1 bi c1 x y z",real=True)
        a = self.l1*sympy.cos(x)+self.rp2*sympy.cos(y)
        a1 = self.l1*sympy.sin(x)+self.rp2*sympy.sin(y)
        funcs = sympy.Matrix([a, a1])
        #print(funcs)
        args = sympy.Matrix([x,y,z])
        res = funcs.jacobian(args)
        print(res)
        return res
    def 主动雅克比改写(self,角度1,角度2,角度3):
        x, y ,z= sympy.symbols("x y z", real=True)
        result = self.主动雅克比.subs([(x,角度1),(y,角度2),(z,角度3)])
        result=result.evalf()
        #g_func = lambdify((5,6),res)
        result=np.array(result)
        #print(result)
        return result
    def 重力矩阵改写(self):
        return np.array([0,-self.m1*self.g,0,-self.m2*self.g,0,-self.m3*self.g])
    def 主动力矩阵改写(self):
        return np.array([-self.fx2,0])
    def 力矩改写乘法(self,角度,角度1,角度2):
        力矩改写乘法=(self.重力矩阵改写().dot(self.改写质量心矩阵(角度,角度1,角度2)).dot(self.约束矩阵(角度,角度1,角度2)))+ \
               (self.主动力矩阵改写().dot(self.主动雅克比改写(角度,角度1,角度2)).dot(self.约束矩阵(角度,角度1,角度2)))+self.力矩矩阵().dot(self.约束矩阵(角度,角度1,角度2))
        return 力矩改写乘法
if __name__=="__main__":
    print('1')
    四连杆=四杆机构等效力矩()
    aa=四连杆.主动雅克比改写(1,2,3)
    bb=四连杆.力矩改写乘法(1,2,3)
    a=四连杆.求解()




